/*
 * @Date: 2020-10-05 16:43:12
 * @LastEditors: Tonser
 * @LastEditTime: 2020-10-19 22:30:05
 * @FilePath: /swarm_ws/src/px4_swarm_control/src/LcmRosNode.cpp
 * @Description: lcmros node
 */
#include <px4_swarm_control/incoming_broadcast_data.h>
#include "LcmRosNode.h"
#include <vector>


LcmRosNode *LcmRosNode::instance = nullptr;
LcmRosNode *LcmRosNode::getInstance() {
    if(instance== nullptr)instance = new LcmRosNode();
    return instance;
}

void
LcmRosNode::setLcmManager(std::string &lcm_url,ros::NodeHandle &nh,bool enable_debug_print) {
    this->enable_debug_print = enable_debug_print;
    lcmManager = lcm::LCM(lcm_url);
    lcmRecvBroadcastDataPub = nh.advertise<px4_swarm_control::incoming_broadcast_data>
            ("lcm_recv_broadcast_data", 1);
    if(!lcmManager.good()){
        ROS_ERROR("LCM %s failed",lcm_url.c_str());
        lcmOk = false;
    }else{
        ROS_INFO("LCM init %s OK!!",lcm_url.c_str());
        lcmOk = true;
    }
    lcmManager.subscribe("SWARM_DATA",&::LcmRosNode::onSwarmDatalcm,this);

}

/***
 * recv lcm data then publish via ros
 * @param receiveBuffer
 * @param channal
 * @param msg
 */
void
LcmRosNode::onSwarmDatalcm(const lcm::ReceiveBuffer* receiveBuffer,
                                const std::string &channal,
                                const SwarmData_t *msg){
    if(enable_debug_print){
        ROS_INFO("lcm recv data buffer : msg %s", std::string(msg->msg_data.begin(),msg->msg_data.end()).c_str());
    }
    ros::Time stamp(msg->sec,msg->nsec);
    px4_swarm_control::incoming_broadcast_data data;
    data.header.stamp = stamp;
    data.lps_time = ros::Time::now().nsec;
    data.remote_id = msg->sender_id;
    data.data = msg->msg_data;
    lcmRecvBroadcastDataPub.publish(data);
}

/**
 * 发送Swarmdata 把打包后的数据直接发送出去
 * @param buffer
 * @param stamp
 * @param id 发送的作者ID
 */
void
LcmRosNode::sendSwarmDataLcm(Buffer buffer,ros::Time stamp,int id){
    SwarmData_t data;
    data.nsec = stamp.nsec;
    data.sec = stamp.sec;
    data.sender_id = id;
    data.msg_data = buffer;
    data.msg_data_length = buffer.size();
    lcmManager.publish("SWARM_DATA",&data);
}

int
LcmRosNode::handle(){
    return lcmManager.handle();

}

// test 
int main(int argc,char **argv){

}

